#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  none,     none,     none)
#pragma config(Sensor, S4,     Gyro,           sensorI2CHiTechnicGyro)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     FrontLeft,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     BackLeft,      tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     FrontRight,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     BackRight,     tmotorTetrix, openLoop, encoder)
#pragma config(Servo,  srvo_S2_C1_1,    FrontLeft,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    BackLeft,             tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    FrontRight,           tServoStandard)
#pragma config(Servo,  srvo_S2_C1_4,    BackRight,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//NOT SURE HOW TO IMPLEMENT FOR SWERVE DRIVE

//HELP FROM MALAVIKA OR JARROD IF HE KNOWS HOW TO USE A SWEREVE DRIVE WOULD BE MUCH APPRECIATED!!!


task main()
{
int leftPower;
int rightPower;
leftPower = 50;
rightPower = 50;

while (true)
{
	if (SensorValue[Gyro] > 610)     //if robot is turning right
	{
		rightPower = rightPower + 1;   //up the right power, counteracting the left lean
	}
	if (SensorValue[Gyro] < 610)		 //if robot is turning left
	{
		rightPower = rightPower - 1;	 //lower the right power, counterating right lean
	}

	motor[Left] = leftPower; 				 //implements the left power
	motor[Right] = rightPower;			 //implements the right power

	wait1Msec(100);									 //waits so that it doesn't increase power as fast as it can process. (total of 200 ms. wait)

		if (SensorValue[Gyro] > 610)	 //if robot is turning right
		{
			leftPower = leftPower - 1;	 //lower the left power, counteracting right lean
		}

		if (SensorValue[Gyro] < 610)	 //if robot is turning left
		{
			leftPower = leftPower + 1;	 //increase left power, counteracting left lean
		}

	motor[Left] = leftPower;				 //implement left power
	motor[Right] = rightPower;			 //implement right power

	wait1Msec(100);									 //wait so it doesn't loop as fast as it can process. (total of 200 ms.)
}

}
